{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "from arm_control import uarm_control\n",
    "import cv2\n",
    "import numpy as np\n",
    "\n",
    "mtx = np.load('./data/camera_param.npz')['mtx']\n",
    "with np.load('./data/eyehand_Matrix.npz') as X:\n",
    "    R, t = [X[i] for i in ('R', 't')]\n",
    "arm = uarm_control.Arm_controller(mtx,R,t)\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "cap = cv2.VideoCapture(4);\n",
    "_, frame = cap.read()\n",
    "cv2.imshow(\"frame\",frame)\n",
    "cv2.waitKey()\n",
    "cv2.destroyAllWindows()\n",
    "cap.release()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[0, array([263.91791528, 164.52182434]), array([[ 4.48666829e-02, -9.98679695e-01, -2.50169423e-02,\n",
      "        -3.78458378e+01],\n",
      "       [ 9.95624194e-01,  4.67561919e-02, -8.09093492e-02,\n",
      "        -3.80486670e+01],\n",
      "       [ 8.19722211e-02, -2.12773389e-02,  9.96407462e-01,\n",
      "         2.31650863e+02],\n",
      "       [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,\n",
      "         1.00000000e+00]])]]\n",
      "识别到id0\n",
      "Control success\n",
      "Control success\n",
      "Control success\n",
      "Control success\n",
      "Control success\n",
      "Control success\n"
     ]
    }
   ],
   "source": [
    "ret,pos = arm.tagpoint(frame,20,0)\n",
    "arm.gate_move(pos,[200,50,10],50)\n",
    "arm.control.reset()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "from vision import transform\n",
    "\n",
    "tf = transform.Transform()\n",
    "\n",
    "tags = tf.get_img2tag(frame,mtx,20)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[[0,\n",
       "  array([263.91791528, 164.52182434]),\n",
       "  array([[ 4.48666829e-02, -9.98679695e-01, -2.50169423e-02,\n",
       "          -3.78458378e+01],\n",
       "         [ 9.95624194e-01,  4.67561919e-02, -8.09093492e-02,\n",
       "          -3.80486670e+01],\n",
       "         [ 8.19722211e-02, -2.12773389e-02,  9.96407462e-01,\n",
       "           2.31650863e+02],\n",
       "         [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,\n",
       "           1.00000000e+00]])]]"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "tags"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "R = tags[0][2]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([-1.22331126, -4.7019381 , 87.41977588])"
      ]
     },
     "execution_count": 17,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "import math\n",
    "tf.rotationMatrixToEulerAngles(R)*180/math.pi"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.10"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
